package roboProject.mainThread;

import java.io.DataInputStream;

import javax.bluetooth.RemoteDevice;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.RFIDSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;
import lejos.robotics.navigation.TachoPilot;
import roboProject.bluetooth.NXTSendingThread;
import roboProject.commandReference.StationCommands;
import roboProject.soundLibrary.SoundFiles;
import roboProject.steering.RoboPilot;

public class RoboProject {

	static BTConnection _StationBTC;
	static RemoteDevice _Station;
	static boolean authenticated = false;
	static RoboPilot _roboPilot;
	static UltrasonicSensor _ultrasonicSensor;
	static LightSensor _lightSensor;
	static RFIDSensor _rfidSensor;
	static DataInputStream dis = null;

	private static NXTSendingThread _NXTSendingThread = new NXTSendingThread();

	public static void main(String[] args) {		
		try {
			_roboPilot = new RoboPilot(new TachoPilot(5.7f, 11.3f, Motor.C, Motor.A));
			_rfidSensor = new RFIDSensor(SensorPort.S1);
			_ultrasonicSensor = new UltrasonicSensor(SensorPort.S2);
			_lightSensor = new LightSensor(SensorPort.S3);
			_lightSensor.setFloodlight(false);
			
			boolean keepLoop = true;
						
			while (keepLoop) {
				if(authenticated){
					byte[] buffer = new byte[1];
					if (dis.read(buffer) >= 0) {
						switch (buffer[0]) {
						case StationCommands.NXT_SHUTDOWN:
							keepLoop = false;
							break;
						case StationCommands.STATION_DISCONNECTED:
							closeBluetoothConnection();
							_roboPilot.goToHome();
							break;
						case StationCommands.START_MOVE_FORWARD:
							_roboPilot.forward();
							break;
						case StationCommands.START_MOVE_BACKWARD:
							_roboPilot.backward();
							break;
						case StationCommands.STOP_MOVE_FORWARD: case StationCommands.STOP_MOVE_BACKWARD: case StationCommands.STOP_TURN_LEFT: case StationCommands.STOP_TURN_RIGHT:
							_roboPilot.floatMotors();
							break;
						case StationCommands.START_TURN_LEFT:
							_roboPilot.turnLeft();
							break;
						case StationCommands.START_TURN_RIGHT:
							_roboPilot.turnRight();
							break;
						case StationCommands.FIRE_CANNON:
							Sound.setVolume(100);
							Sound.playSample(SoundFiles.HELLO);
							Sound.setVolume(0);
							break;
						case StationCommands.ACTIVATE_FLOODLIGHT:
							_lightSensor.setFloodlight(true);
							break;
						case StationCommands.DEACTIVATE_FLOODLIGHT:
							_lightSensor.setFloodlight(false);
							break;
						default:
							Sound.setVolume(100);
							Sound.beepSequence();
							Sound.setVolume(0);
							break;
						}
					}
				} else {
					LCD.drawString("RoboProject", 1, 1);
					LCD.drawString("is locked!", 1, 2);
					LCD.drawString("activate RFID..", 1, 3);
					
					while (!authenticated) {
						if(_rfidSensor.readTransponderAsLong(false) != 0){
							authenticated = true;
							LCD.clear();
							LCD.drawString("Waiting 60 sec.", 1, 1);
							LCD.drawString("for connection..", 1, 2);
							_StationBTC = Bluetooth.waitForConnection(60000, NXTConnection.RAW);

							if (_StationBTC == null) {
								LCD.clear();
								LCD.drawString("No Bluetooth", 1, 1);
								LCD.drawString("connected!", 1, 2);
								LCD.drawString("exiting..", 1, 3);
								Button.waitForPress();
								return;
							}
							_Station = RemoteDevice.getRemoteDevice(_StationBTC);
							
							dis = _StationBTC.openDataInputStream();

							LCD.clear();
							LCD.drawString("RoboGuide", 1, 1);
							_NXTSendingThread = new NXTSendingThread();
							_NXTSendingThread.setOutputStream(_StationBTC.openDataOutputStream());
							_NXTSendingThread.set_roboPilot(_roboPilot);
							_NXTSendingThread.set_ultrasonicSensor(_ultrasonicSensor);
							_NXTSendingThread.set_lightSensor(_lightSensor);
							_NXTSendingThread.set_stationConnection(_StationBTC);
							_NXTSendingThread.start();
						}
						if (Button.ESCAPE.isPressed()) {
							return;
						}
					}
				}
			}
		} catch (Exception e) {
			LCD.clear();
			System.out.println("An Error Occured!");
			Button.waitForPress();
		} finally {
			LCD.clear();
			System.out.println("Closing connections..");
			try {
				closeBluetoothConnection();
			} catch (Exception e) {
			}
		}
	}
	
	private static void closeBluetoothConnection(){
		try {
			if (_NXTSendingThread.isAlive()) {
				_NXTSendingThread.setShouldStop();
			}
			dis.close();
			Thread.sleep(100);
			_StationBTC.close();
			authenticated = false;
		} catch (Exception e) {
		}
	}

}
